import cv2 as cv
import numpy as np
import time
import threading
import json
import os

class USBCamStream:
    def __init__(self, vid=0):
        # USB摄像头id
        self.vid = vid
        # 打开摄像头
        self.vcap = cv.VideoCapture(self.vid + cv.CAP_V4L2)
        if not self.vcap.isOpened():
            print("[Exiting]: Error accessing usbcam stream.")
            exit(0)
        # 整除2的原因是/dev/video2对应于cv.VideoCapture(1)
        print('Cam_{} init success'.format(self.vid//2))

        self.width = 480 #640 #1280 #1920  # 定义摄像头获取图像宽度
        self.height = 480 #960 #1080  # 定义摄像头获取图像长度
        self.fps = 60 # 定义拍摄视频的帧率 默认60fps
        self.vcap.set(6, cv.VideoWriter_fourcc('M', 'J', 'P', 'G')) # 设置图像的编码格式
        self.vcap.set(cv.CAP_PROP_FPS, self.fps)  # 设置帧率
        self.vcap.set(cv.CAP_PROP_FRAME_WIDTH, self.width)  # 设置宽度
        self.vcap.set(cv.CAP_PROP_FRAME_HEIGHT, self.height)  # 设置高度
        # 记录硬件视频帧率
        self.fps_input_stream = int(self.vcap.get(cv.CAP_PROP_FPS))
        # 测试图像捕获
        self.ret, self.frame = self.vcap.read()
        if not self.ret:
            print('[Exiting] No more frames to read')
            exit(0)
        
        # 加载摄像头内参
        dirname = os.path.dirname(__file__)
        filename = os.path.join(dirname, "camera-params-IR.json")
        f = open(filename)
        self.camera_params = json.load(f)

        # 定义HSV阈值，使mask清晰，精确
        self.hsv_low = np.array([  0, 0,149])
        self.hsv_high= np.array([180,39,255])
        # 所有点的坐标[[x,y],]
        self.image_points = []
        # 线程退出标志位
        self.thread_stoped = False
        # 定义线程函数与参数
        self.process_thread = threading.Thread(target=self.update, args=())
        # 设置线程为守护线程，不能独立于主线程
        self.process_thread.daemon = True
        self.process_thread.start()
    
    # 通过.json文件获取红外摄像头内参
    def get_camera_params(self, camera_num):
        return {
            "intrinsic_matrix": np.array(self.camera_params[camera_num]["intrinsic_matrix"]),
            "distortion_coef": np.array(self.camera_params[camera_num]["distortion_coef"]),
            "rotation": self.camera_params[camera_num]["rotation"]
        }
    
    # 线程函数，进行图像捕获、处理、并求出点坐标
    def update(self):
        while not self.thread_stoped:
            ret, frame = self.vcap.read()
            if not ret:
                 print('[Exiting] No more frames to read')
                 break
            
            # 清空所有的点，准备下一帧图像
            image_all_points = []

            '''图像处理程序'''
            frame = cv.cvtColor(frame, cv.COLOR_RGB2HSV) # 转为HSV色彩空间
            mask = cv.inRange(frame, self.hsv_low, self.hsv_high)  # 通过HSV的高低阈值，提取图像部分区域，输出的mask为二值图像
            mask = np.rot90(mask, k=self.camera_params[self.vid//2]["rotation"])
            mask = cv.undistort(mask, self.get_camera_params(self.vid//2)["intrinsic_matrix"], self.get_camera_params(self.vid//2)["distortion_coef"])
            # 调用make_square方法将图像转化为外接正方形
            mask = self.make_square(mask)
            # 调用_find_dot返回画出轮廓和每个点坐标的图像和当前图像中所有的点坐标
            mask, image_all_points = self._find_dot(mask)
            # 保存到成员变量中
            self.frame = mask
            self.image_points = image_all_points

    # 找到发光点坐标
    def _find_dot(self, img):
        # 发光点轮廓检测
        contours = cv.findContours(img, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)[1]
        # 画出所有轮廓
        img = cv.drawContours(img, contours, -1, 255, 1) # (0,255,0)

        image_points = []
        for contour in contours:
            # 根据轮廓求矩
            moments = cv.moments(contour)
            if moments["m00"] != 0:
                # 轮廓contour的质心的横、纵坐标
                center_x = int(moments["m10"] / moments["m00"])
                center_y = int(moments["m01"] / moments["m00"])
                cv.putText(img, f'({center_x}, {center_y})', (center_x,center_y - 15), cv.FONT_HERSHEY_SIMPLEX, 0.3, 255, 1)
                cv.circle(img, (center_x,center_y), 1, 255, -1)
                image_points.append([center_x, center_y])

        if len(image_points) == 0:
            image_points = [[None, None]]

        return img, image_points
    
    # 将图像转化为外接正方形，将原始图像放在中间
    def make_square(self, img):
        x, y = img.shape
        size = max(x, y)
        new_img = np.zeros((size, size), dtype=np.uint8)
        ax,ay = (size - img.shape[1])//2,(size - img.shape[0])//2
        new_img[ay:img.shape[0]+ay,ax:ax+img.shape[1]] = img
        # Pad the new_img array with edge pixel values
        # Apply feathering effect
        feather_pixels = 8
        for i in range(feather_pixels):
            alpha = (i + 1) / feather_pixels
            new_img[ay - i - 1, :] = img[0, :] * (1 - alpha)  # Top edge
            new_img[ay + img.shape[0] + i, :] = img[-1, :] * (1 - alpha)  # Bottom edge
        return new_img
    
    # 读取一帧处理好的图像和计算出的所有的点
    def read_fram_points(self):
        return (self.frame, self.image_points)
    
    # 停止线程、释放资源
    def stop(self):
        self.thread_stoped = True
        # 等待线程结束
        self.process_thread.join()
        # 释放资源，关闭窗口
        self.vcap.release()
        cv.destroyAllWindows()


def main():
    # usb_cam = [USBCamStream(vid=0), USBCamStream(vid=2)]
    usb_cam = []
    usb_cam.append(USBCamStream(vid=0))
    usb_cam.append(USBCamStream(vid=2))
    usb_cam.append(USBCamStream(vid=4))
    usb_cam.append(USBCamStream(vid=6))
    now_time, last_time= time.time(), time.time()
    while True:
        time1 = time.time()
        last_time = now_time
        now_time=time.time() #记录开始处理之前
        p = now_time - last_time
        fps = 1 / p
        frame,_ = usb_cam[0].read_fram_points()
        frame1,_ = usb_cam[1].read_fram_points()
        frame2,_ = usb_cam[2].read_fram_points()
        frame3,_ = usb_cam[3].read_fram_points()
        if usb_cam[0].thread_stoped:
            break
        cv.putText(frame, "{:.1f}".format(fps), (10, 30), cv.FONT_HERSHEY_SIMPLEX, 1, 255, 2, cv.LINE_AA)
        cv.imshow('frame', frame)
        cv.putText(frame1, "{:.1f}".format(fps), (10, 30), cv.FONT_HERSHEY_SIMPLEX, 1, 255, 2, cv.LINE_AA)
        cv.imshow('frame1', frame1)
        cv.putText(frame2, "{:.1f}".format(fps), (10, 30), cv.FONT_HERSHEY_SIMPLEX, 1, 255, 2, cv.LINE_AA)
        cv.imshow('frame2', frame2)
        cv.putText(frame3, "{:.1f}".format(fps), (10, 30), cv.FONT_HERSHEY_SIMPLEX, 1, 255, 2, cv.LINE_AA)
        cv.imshow('frame3', frame3)
        time2 = time.time()
        # 控制帧率
        if cv.waitKey(int(15-(time2-time1))) == ord('q'):
            break
    usb_cam[0].stop()
    usb_cam[1].stop()
    usb_cam[2].stop()
    usb_cam[3].stop()
    



if __name__ == '__main__':
    main()

